#include <stdio.h>
#include <unistd.h>
#include "qegpioint.h"
#include "RCTest.h"

extern CQEGpioInt &gpio;

static unsigned long diff(struct timeval *ptv0, struct timeval *ptv1)
{
  long val;

  val = ptv1->tv_usec - ptv0->tv_usec;
  val += (ptv1->tv_sec - ptv0->tv_sec)*1000000;

  return val;
}

void rcTestCallback(unsigned int io, struct timeval *ptv, void *userData)
{
	int pw;
	struct RCChannel *rcc_p;

	rcc_p = (struct RCChannel *)userData;
	pw = diff(&(rcc_p->tv0), ptv);
	rcc_p->tv0 = *ptv;
	if (rcc_p->flag) {
		rcc_p->flag = 0;
		gpio.SetInterruptMode(rcc_p->dioIndex, QEG_INTERRUPT_NEGEDGE);

	} else {
		//printf("%d\n", pw);
		rcc_p->lastPulseWidth = rcc_p->currentPulseWidth;	// save away the last value for use in the filter

		// if no pulses, set it to neutral
		if (pw < 2500) {
			rcc_p->currentPulseWidth = pw;
			//printf("Got RC pulse on index %d, width: %d\n", rcc_p->dioIndex, rcc_p->currentPulseWidth);
			rcc_p->flag = 1;
			gpio.SetInterruptMode(rcc_p->dioIndex, QEG_INTERRUPT_POSEDGE);
			return;
		} else {
			//rcc_p->currentPulseWidth = 1500;
			//printf("WARNING: no RC pulse on index %d, setting to 1500\n", rcc_p->dioIndex);
		}

		// filter: output the value out of the last two that's closer to 1500, to remove noise
		if (abs(rcc_p->currentPulseWidth-1500) > abs(rcc_p->lastPulseWidth-1500)) {
			rcc_p->pulseWidth = rcc_p->lastPulseWidth;
			//printf("dropped possibly noisy reading %d: ", rcc_p->currentPulseWidth);
		} else {
			rcc_p->pulseWidth = rcc_p->currentPulseWidth;
		}
		rcc_p->pulseWidth = rcc_p->currentPulseWidth;
		rcc_p->flag = 1;
		gpio.SetInterruptMode(rcc_p->dioIndex, QEG_INTERRUPT_POSEDGE);
	}
}

struct RCChannel * initRCTest(CQEGpioInt& gpio, int dioPortIn)
{
  // malloc the channel control struct & initialize it
  struct RCChannel * rcc_p = (struct RCChannel *)malloc(sizeof(RCChannel));
  rcc_p->flag = 1;
  rcc_p->dioIndex = dioPortIn - 1;

  // set up the channel DIO to be an input & attach interrupt callback
  unsigned int dataDirection = gpio.GetDataDirection();
  dataDirection = dataDirection & ~(1<<(rcc_p->dioIndex));
  gpio.SetDataDirection(dataDirection);
  gpio.RegisterCallback(rcc_p->dioIndex, (void *)rcc_p, rcTestCallback);
  gpio.SetInterruptMode(rcc_p->dioIndex, QEG_INTERRUPT_POSEDGE);

  return rcc_p;
}

